#include <istream>
#include <stdio.h>
#include <string>
#include <vector>
#include <fstream>
#include <ostream>
#include <stdio.h>
#include <stdlib.h>
#include <list>
#include <algorithm>

#include "base/cmvs/bundle.h"
#include "genOption.h"


void inputData::getbundleFile(const string in)
{
    std::ifstream ifstr;  ifstr.open(in.c_str());
    if (!ifstr.is_open()) {
        std::cerr << "images file not found: " << in << "\n";
        exit(1);
    }

    while (1) {
        unsigned char uctmp;
        ifstr.read((char*)&uctmp, sizeof(unsigned char));
        ifstr.putback(uctmp);
        if (uctmp == '#') {
            char buffer[1024];      ifstr.getline(buffer, 1024);
        }
        else
            break;
    }

    int cnum, pnum;
    ifstr >> cnum >> pnum;

    m_bundle.m_coords.clear();
    m_bundle.m_colors.clear();
    m_bundle.m_visibles.clear();

    std::vector<int> ids;        ids.resize(cnum);
    m_bundle.m_cnum = 0;

    for (int c = 0; c < cnum; ++c) {
        ids[c] = -1;
        float params[15];

        Eigen::MatrixXd r(3,3);
        Eigen::MatrixXd t(3,1);
        Eigen::MatrixXd se3(3,4);

        for (int i = 0; i < 15; i++){
            ifstr >> params[i];
        }

        if (params[0] != 0.0f)
            ids[c] =m_bundle.m_cnum++;

        r<<params[3],params[4],params[5],params[6],params[7],
                params[8],params[9],params[10],params[11];
        t<<params[12],params[13],params[14];

        r=r.inverse();
        t=-r*t;

        se3<<r(0,0),r(0,1),r(0,2),t(0),r(1,0),r(1,1),r(1,2),t(1),r(2,0),r(2,1),r(2,2),t(2);
        m_rtran.push_back(se3);

    }

    m_bundle.m_pnum = 0;
    m_bundle.m_vpoints.resize(m_bundle.m_cnum);
    for(int j=0;j<pnum;j++){
        int num;
        Vec3f color;
        Vec4f coord;

        ifstr>>coord[0] >> coord[1] >> coord[2] >> color >> num;
        coord[3]=1.0f;

        std::vector<int> visibles;
        for(int i=0;i<num;++i){
            int itmp;      ifstr >> itmp;

            if(cnum<=(itmp-1)){
                double dtmp,tmp;
                ifstr >> itmp >> dtmp >> tmp;
                continue;
            }

            if (ids[itmp-1] == -1) {
                std::cerr << "impossible " << itmp << ' ' << ids[itmp-1] << "\n";
                exit (1);
            }

            visibles.push_back(ids[itmp-1]);

            double dtmp,tmp;
            ifstr >> itmp >> dtmp >> tmp;
        }

        if ((int)visibles.size() < 2)
            continue;

        std::sort(visibles.begin(), visibles.end());

        for (int i = 0; i < (int)visibles.size(); ++i) {
            m_bundle.m_vpoints[visibles[i]].push_back(m_bundle.m_pnum);
        }

        m_bundle.m_visibles.push_back(visibles);
        m_bundle.m_colors.push_back(color);
        m_bundle.m_coords.push_back(coord);

        ++m_bundle.m_pnum;
    }
}

void inputData::getbundleFile(std::vector<Eigen::MatrixXd> r, std::vector<Eigen::MatrixXd> t, std::vector<std::vector<int> > vpoints,
                              std::vector<std::vector<int> > visibles, std::vector<Vec3f> colors, std::vector<Vec4f> coords)
{
    Eigen::MatrixXd se3(3,4);
    for(int i=0;i<r.size();i++){
        r[i]=r[i].inverse();
        t[i]=-r[i]*t[i];
        se3<<r[i](0,0),r[i](0,1),r[i](0,2),t[i](0),r[i](1,0),r[i](1,1),r[i](1,2),t[i](1),r[i](2,0),r[i](2,1),r[i](2,2),t[i](2);
        m_rtran.push_back(se3);
    }



    m_bundle.m_visibles=visibles;
    m_bundle.m_colors=colors;
    m_bundle.m_coords=coords;
    m_bundle.m_vpoints=vpoints;
}

void inputData::getImage(const std::string in,  const std::string out)
{

    char imagein[1024];
    char imageout[1024];

    sprintf(imagein,"%s/images.txt",in.c_str());
    sprintf(imageout,"%svisualize",out.c_str());


    std::ifstream ifstr;  ifstr.open(imagein);
    if (!ifstr.is_open()) {
        std::cerr << "images file not found: " << imagein << "\n";
        exit(1);
    }

    std::ifstream ifout;  ifout.open(imageout);
    if(!ifout.is_open())
    {
        char  cmds[2048];
        sprintf(cmds, "mkdir -p '%s'", imageout);
        system(cmds);
    }

//        string imageName=imageprefix+"/"+image;
//        cv::Mat dstImage=cv::imread(imageName);
//        if(!dstImage.empty())
//            continue;

    int i=0;
    char buffer[1024];
    char image[1024];

    while(!(ifstr.eof()))
    {
        cv::Mat dst;
        cv::Mat src;
        sprintf(image,"%08d.jpg",i);
        std::string imageprefix=imageout;
        string imageName=imageprefix+"/"+image;

        ifstr.getline(buffer,sizeof(buffer));
        string filename=buffer;
        string a;

        if(filename.substr(0,1)!=".")
        {
            src=cv::imread(filename);
            if(src.empty())
            {
                std::cout<<"the image is empty!\n";
            }
            else{
            cv::imshow("1",src);
            cv::waitKey();
            }

        }
        else{
            int pos1=filename.find_first_of('/');
            if(filename.substr(1,1)!="."){
                a=filename.substr(pos1,filename.size());

                string b=in+a;
                std::cout<<"image.name:"<<b<<endl;
                src=cv::imread(b);
                if(src.empty())
                {
                    std::cout<<"the image is empty!\n";
                }
//                else{
//                cv::imshow("1",src);
//                cv::waitKey();
//                }
            }

            else if(filename.substr(pos1,1) != "."){
                int pos=in.find_last_of('/');
                string b=in.substr(0,pos);
                a=filename.substr(pos1,filename.size());
                src=cv::imread(b+a);
            }
            else if(filename.substr(pos1,1) == "."){
                a=filename.substr(pos1,filename.size());
                int pos2=a.find_first_of('/');
                a=a.substr(pos2,filename.size());
                int pos=in.find_last_of('/');
                string b=in.substr(0,pos);
                pos=b.find_last_of("/");
                b=b.substr(0,pos);
                std::cout<<b+a<<"\n";
                src=cv::imread(b+a);
            }
        }

        if(src.empty())
            break;

        scale=(double)2400/src.cols;


        //        cout<<"image:"<<imageName<<"\n";
        cv::Mat dstImage=cv::imread(imageName);
        if(!dstImage.empty()){
            i++;
            continue;
        }
        cv::resize(src,dst,cv::Size(src.cols*scale,src.rows*scale),0,0);

        cv::imwrite(imageName,dst);

        i++;
    }

}

void inputData::getInputFile(const std::string orimage,const std::string prefix)
{
    char buffer0[1024];
    char buffer1[1024];

    sprintf(buffer0, "%s/bundle.rd.out", orimage.c_str());
    sprintf(buffer1,"%s/camera.txt",orimage.c_str());


    getImage(orimage,prefix);
    getbundleFile(buffer0);
    readCameraFile(buffer1);
    computeP();
}

std::vector<Eigen::MatrixXd> &inputData::getP()
{
    return m_pose;
}

void inputData::readCameraFile(const std::string file)
{
    std::ifstream ifstr;  ifstr.open(file.c_str());
    if (!ifstr.is_open()) {
        std::cerr << "camera file not found: " << file << "\n";
        exit (1);
    }

    char str[1024];
    double param;
    std::vector<double> params;

    while (!ifstr.eof()) {
        ifstr>>str;
        if(!isdigit(str[0]))
        {
            for(int i=0;i<strlen(str);i++)
                str[i]=str[i+1];
        }
        if(!isdigit(str[strlen(str)-1]))
        {
            str[strlen(str)-1]='\0';
        }

        std::stringstream stream;
        stream<<str;
        stream>>param;
        params.push_back(param);
    }

    if(params.size()<7 || params.size()>9)
    {
        //        Eigen::MatrixXd k(3,3);
        if(scale==1) {
            m_camera<<params[2],0,params[4],0,params[3],params[5],0,0,1;
        }

        params[2]=params[2]*scale;
        params[3]=params[3]*scale;
        params[4]=params[4]*scale;
        params[5]=params[5]*scale;

        m_camera<<params[2],0,params[4],0,params[3],params[5],0,0,1;
    }

    if(params.size()==9)
    {
        if(scale==1){
            m_camera<<params[0],params[1],params[2],params[3],params[4],params[5],
                    params[6],params[7],params[8];
        }

        params[0]=params[0]*scale;
        params[4]=params[4]*scale;
        params[2]=params[2]*scale;
        params[5]=params[5]*scale;

        m_camera<<params[0],params[1],params[2],params[3],params[4],params[5],
                params[6],params[7],params[8];
    }

    ifstr.close();
}

void inputData::readCameraFile(std::vector<double> camerap)
{
    if(camerap.empty()){
        std::cout<<"the camerap is empty!\n";
        exit(1);
    }

    if(camerap.size()<7 || camerap.size()>9)
    {
        //        Eigen::MatrixXd k(3,3);
        if(scale==1) {
            m_camera<<camerap[2],0,camerap[4],0,camerap[3],camerap[5],0,0,1;
        }

        camerap[2]=camerap[2]*scale;
        camerap[3]=camerap[3]*scale;
        camerap[4]=camerap[4]*scale;
        camerap[5]=camerap[5]*scale;

        m_camera<<camerap[2],0,camerap[4],0,camerap[3],camerap[5],0,0,1;
    }

    if(camerap.size()==9)
    {
        if(scale==1){
            m_camera<<camerap[0],camerap[1],camerap[2],camerap[3],camerap[4],camerap[5],
                    camerap[6],camerap[7],camerap[8];
        }

        camerap[0]=camerap[0]*scale;
        camerap[4]=camerap[4]*scale;
        camerap[2]=camerap[2]*scale;
        camerap[5]=camerap[5]*scale;

        m_camera<<camerap[0],camerap[1],camerap[2],camerap[3],camerap[4],camerap[5],
                camerap[6],camerap[7],camerap[8];
    }
}

float inputData::computeP()
{
    m_bundle.m_intri.resize(m_bundle.m_cnum);
    m_bundle.m_extri.resize(m_bundle.m_cnum);

    for (int index = 0; index < m_bundle.m_cnum; ++index){
        Eigen::MatrixXd p(3,4);
        p=m_camera*m_rtran[index];

        m_bundle.m_intri[index].push_back(p(0,0));
        m_bundle.m_intri[index].push_back(p(0,1));
        m_bundle.m_intri[index].push_back(p(0,2));
        m_bundle.m_intri[index].push_back(p(0,3));
        m_bundle.m_intri[index].push_back(p(1,0));
        m_bundle.m_intri[index].push_back(p(1,1));

        m_bundle.m_extri[index].push_back(p(1,2));
        m_bundle.m_extri[index].push_back(p(1,3));
        m_bundle.m_extri[index].push_back(p(2,0));
        m_bundle.m_extri[index].push_back(p(2,1));
        m_bundle.m_extri[index].push_back(p(2,2));
        m_bundle.m_extri[index].push_back(p(2,3));

        m_pose.push_back(p);
    }
}

void inputData::getallFile(string path)
{

}

CMVS::Cbundle &inputData::getBundle()
{
    return m_bundle;
}


